package kit.edu.lego.gruppe3.actions;

import kit.edu.lego.gruppe3.sensors.GroundType;
import kit.edu.lego.gruppe3.sensors.Scanner;

public class Orientation {
	public static boolean orientAtLine(){
		return orientAtType(GroundType.Line);
	}
	public static boolean orientAtType(GroundType type) {
		if (Scanner.getFirstOf(type) < -200)
			return false;
		int first;
		int last;
		while (Math.abs((first = Scanner.getFirstOf(type, true))
				+ (last = Scanner.getLastOf(type))) > 15) {
			int diff = first + last;
			if (Math.abs(first - last) < 15) {
				Movement.turnCenter(first);
			} else {
				Movement.turnCenter(diff / 2);
			}
		}
		return true;
	}

	public static boolean orientAtWall() {
		double diff;
		int rangeL;
		int rangeR;
		do {
			rangeL = 0;
			rangeR = 0;
			for (int i = 0; i < 10; i++)
				if (Scanner.getRangeAt(-20, true) < 250)
					rangeL += Scanner.getRangeAt(-20);
			for (int i = 0; i < 10; i++)
				if (Scanner.getRangeAt(20, true) < 250)
					rangeR += Scanner.getRangeAt(20, true);
			if (rangeL > 0 && rangeR > 0) {
				diff = rangeL - rangeR;
			} else if (rangeL > 0) {
				diff = -225;
			} else if (rangeR > 0) {
				diff = 255;
			} else
				diff = 0;
			Movement.turnCenter((int) (diff / 5));
		} while (Math.abs(diff) > 50);

		return true;
	}

	public static boolean centerOnLine() {
		return centerOnLine(true);
	}

	public static boolean centerOnLine(boolean isOnLine) {
		if (!isOnLine && !orientAtLine())
			return false;
		if (!isOnLine) {
			int angle = Scanner.getFirstLine();
			Movement.travel(Math.cos(angle) * 4 + 2, 10);
		}
		Movement.turnCenter(90);
		int angleL = Scanner.getFirstLine(true);
		int angleR = Scanner.getLastLine();
		while (angleL < -200) {
			Movement.travel(-2.5, 10);
			if (Math.abs(angleL) < Math.abs(angleR) && Math.abs(angleL) < 40)
				Movement.turnCenter(angleL / 3);
			else if (Math.abs(angleR) < 40)
				Movement.turnCenter(angleR / 3);
			angleL = Scanner.getFirstLine(true);
			angleR = Scanner.getLastLine();
		}
		while (angleL > -200) {
			Movement.travel(2.5, 10);
			if (Math.abs(angleL) < Math.abs(angleR) && Math.abs(angleL) < 40)
				Movement.turnCenter(angleL / 3);
			else if (Math.abs(angleR) < 40)
				Movement.turnCenter(angleR / 3);
			angleL = Scanner.getFirstLine(true);
			angleR = Scanner.getLastLine();
		}
		Movement.turnCenter(180);
		int k = 0;
		angleL = Scanner.getFirstLine(true);
		while (angleL > -200) {
			k++;
			Movement.travel(2.5, 10);
			if (Math.abs(angleL) < Math.abs(angleR) && Math.abs(angleL) < 40)
				Movement.turnCenter(angleL / 3);
			else if (Math.abs(angleR) < 40)
				Movement.turnCenter(angleR / 3);
			angleL = Scanner.getFirstLine(true);
			angleR = Scanner.getLastLine();
		}
		Movement.travel(-k * 5 / 4, 10);
		Movement.turnCenter(90);
		return true;
	}
}
